#include <Arduino.h>
#include "simpleMath.h"

//俯仰速率最大值
#define prMax 100
#define prMin -80
//AOA最大值
#define aoaMax 25
#define aoaMin -18
//横滚速率最大值
#define rrMax 100
//偏航速率最大值
#define yrMax 50
//俯仰角度最大值
#define pMax 45
#define pMin -45
//横滚角度最大值
#define rMax 45

//内环横滚速率PID
#define rrP 0.6
#define rrI 0.0
#define rrD 0.0
//俯仰速率PID
#define prP 0.6
#define prI 0.0
#define prD 0.0
//偏航速率PID
#define yrP 0.0
#define yrI 0.0
#define yrD 0.0

//内环俯仰速率PID_2，无攻角控制模式下的速率PID
#define prP2 0.6
#define prI2 0.0
#define prD2 0.0

//内环AOA控制PID
#define paP 0.7
#define paI 0.0
#define paD 0.0

//横滚角度PID
#define rP 1.0
#define rI 0.0
#define rD 0.0
//俯仰角度PID
#define pP 1.0
#define pI 0.0
#define pD 0.0

void PID();
float PID_AOA(float in,float fb,float p,float i,float d,bool rOUT);//攻角控制器
float PID_PR(float in,float fb,float p,float i,float d,bool rOUT);//俯仰速率控制器
float PID_RR(float in,float fb,float p,float i,float d,bool rOUT);//横滚速率控制器
float PID_YR(float in,float fb,float p,float i,float d,bool rOUT);//偏航速率控制器
